/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.drive;

import com.team1160.breakaway.api.Constants;
import com.team1160.breakaway.model.Model;
import com.team1160.breakaway.model.ModelNotifier;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Jaguar;

/**
 *
 * @author CJ
 */
public class Drive implements ModelNotifier{
    //TODO MANGO

    protected double maxSpeed = Constants.SPEED_MAX_NOMINAL;

    protected final Jaguar J_LEFT_BACK = new Jaguar(Constants.C_DT_LEFT_BACK);
    protected final Jaguar J_LEFT_FRONT = new Jaguar(Constants.C_DT_LEFT_FRONT);
    protected final Jaguar J_RIGHT_BACK = new Jaguar(Constants.C_DT_RIGHT_BACK);
    protected final Jaguar J_RIGHT_FRONT = new Jaguar(Constants.C_DT_RIGHT_FRONT);

    protected boolean e_left_back;
    protected boolean e_left_front;
    protected boolean e_right_back;
    protected boolean e_right_front;

    protected final Encoder E_LEFT_BACK =
                new Encoder(Constants.C_ENC_DT_LEFT_BACK_A,
                            Constants.C_ENC_DT_LEFT_BACK_B);
    protected final Encoder E_LEFT_FRONT =
                new Encoder(Constants.C_ENC_DT_LEFT_FRONT_A,
                            Constants.C_ENC_DT_LEFT_FRONT_B);
    protected final Encoder E_RIGHT_BACK = new Encoder(Constants.C_ENC_DT_RIGHT_BACK_A,
            Constants.C_ENC_DT_RIGHT_BACK_B);
    protected final Encoder E_RIGHT_FRONT = new Encoder(Constants.C_ENC_DT_RIGHT_FRONT_A,
            Constants.C_ENC_DT_RIGHT_FRONT_B);

    public Drive()
    {   
        this.e_left_back = true;
        this.e_left_front = true;
        this.e_right_back = true;
        this.e_right_front = true;
        initEncoders();
        initJags();
    }

    protected void initJags(){
        this.J_LEFT_BACK.setBounds(Constants.JAGUAR_MAX_PWN,
                Constants.JAGUAR_UP_DEAD_PWN,
                Constants.JAGUAR_MID_PWN,
                Constants.JAGUAR_DOWN_DEAD_PWN,
                Constants.JAGUAR_MIN_PWN);
        this.J_LEFT_FRONT.setBounds(Constants.JAGUAR_MAX_PWN,
                Constants.JAGUAR_UP_DEAD_PWN,
                Constants.JAGUAR_MID_PWN,
                Constants.JAGUAR_DOWN_DEAD_PWN,
                Constants.JAGUAR_MIN_PWN);
        this.J_RIGHT_BACK.setBounds(Constants.JAGUAR_MAX_PWN,
                Constants.JAGUAR_UP_DEAD_PWN,
                Constants.JAGUAR_MID_PWN,
                Constants.JAGUAR_DOWN_DEAD_PWN,
                Constants.JAGUAR_MIN_PWN);
        this.J_RIGHT_FRONT.setBounds(Constants.JAGUAR_MAX_PWN,
                Constants.JAGUAR_UP_DEAD_PWN,
                Constants.JAGUAR_MID_PWN,
                Constants.JAGUAR_DOWN_DEAD_PWN,
                Constants.JAGUAR_MIN_PWN);

    }

    protected void initEncoders(){
        this.E_LEFT_BACK.setDistancePerPulse(Constants.ENCODER_DISTANCE_PER_PULSE);
        this.E_LEFT_FRONT.setDistancePerPulse(Constants.ENCODER_DISTANCE_PER_PULSE);
        this.E_RIGHT_BACK.setDistancePerPulse(Constants.ENCODER_DISTANCE_PER_PULSE);
        this.E_RIGHT_FRONT.setDistancePerPulse(Constants.ENCODER_DISTANCE_PER_PULSE);

        this.E_LEFT_BACK.start();
        this.E_LEFT_FRONT.start();
        this.E_RIGHT_BACK.start();
        this.E_RIGHT_FRONT.start();
    }

    public void updateModel(Model model){
        this.updateMotorSpeeds(model);
    }

    protected void updateMotorSpeeds(Model model){
        model.setDriveTrainMotorValue(Constants.ID_DRIVETRAIN_LEFT_BACK,
                this.E_LEFT_BACK.getRate());
        model.setDriveTrainMotorValue(Constants.ID_DRIVETRAIN_LEFT_FRONT,
                this.E_LEFT_FRONT.getRate());
        model.setDriveTrainMotorValue(Constants.ID_DRIVETRAIN_RIGHT_BACK,
                this.E_RIGHT_BACK.getRate());
        model.setDriveTrainMotorValue(Constants.ID_DRIVETRAIN_RIGHT_FRONT,
                this.E_RIGHT_FRONT.getRate());
    }

    public void setMaxSpeed(double maxSpeed)
    {
        if (maxSpeed >= 0.0)
            this.maxSpeed = maxSpeed;
        else
            this.maxSpeed = -1 * maxSpeed;
    }

    public double getMaxSpeed()
    {
        return this.maxSpeed;
    }

    public void setLeftRightMotorSpeeds(double leftSide, double rightSide){
        leftSide = limit(leftSide);
        rightSide = limit(rightSide);

        if(this.e_left_back)
            this.J_LEFT_BACK.set(-leftSide * 22/28);
        else
            this.J_LEFT_BACK.set(0.0);

        if(this.e_left_front)
            this.J_LEFT_FRONT.set(-leftSide);
        else
            this.J_LEFT_FRONT.set(0.0);

        if(this.e_right_back)
            this.J_RIGHT_BACK.set(rightSide * 22/28);
        else
            this.J_RIGHT_BACK.set(0.0);

        if(this.e_right_front)
            this.J_RIGHT_FRONT.set(rightSide);
        else
            this.J_RIGHT_FRONT.set(0.0);
    }

    private double limit(double input){
        if(input < - this.maxSpeed)
            input = - this.maxSpeed;
        if(input > this.maxSpeed)
            input = this.maxSpeed;
        return input;
    }

    public void motorStop(byte motorID)throws IllegalArgumentException{
       //if(motorID > Constants.ID_DRIVETRAIN_RIGHT_BACK || motorID < Constants.ID_DRIVETRAIN_LEFT_FRONT)
        //    throw new IllegalArgumentException("Invalid MotorID");

        if(motorID == Constants.ID_DRIVETRAIN_LEFT_BACK)
            this.e_left_back = false;
        else
            this.e_left_back = true;
        
        if(motorID == Constants.ID_DRIVETRAIN_LEFT_FRONT)
            this.e_left_front = false;
        else
            this.e_left_front = true;
        
        if(motorID == Constants.ID_DRIVETRAIN_RIGHT_BACK)
            this.e_right_back = false;
        else
            this.e_right_back = true;
        
        if(motorID == Constants.ID_DRIVETRAIN_RIGHT_FRONT)
            this.e_right_front = false;
        else
            this.e_right_front = true;
    }
}
